#include "HOG.h"

HOG::HOG(void)
{
}

HOG::~HOG(void)
{
}

/*Function to calculate the integral histogram*/
IplImage** HOG::calculateIntegralHOG(IplImage* inImg, int nbins)
{
	/* By convention, the orientation range is {0, 180} */
	float minDegree = 180. / nbins;

	/*Convert the input image to grayscale*/
	IplImage* img_gray = cvCreateImage(cvGetSize(inImg), IPL_DEPTH_8U,1);
	cvCvtColor(inImg, img_gray, CV_BGR2GRAY);
	cvEqualizeHist(img_gray,img_gray);

	/* Calculate the derivatives of the grayscale image in the x and y directions 
	using a sobel operator and obtain 2 gradient images for the x and y directions */
	IplImage *xsobel = cvCreateImage(cvGetSize(inImg), IPL_DEPTH_32F,1);
	IplImage *ysobel = cvCreateImage(cvGetSize(inImg), IPL_DEPTH_32F,1);	
	IplImage* drvX = cvCreateImage( cvGetSize( inImg ), IPL_DEPTH_16S, 1 );
	IplImage* drvY = cvCreateImage( cvGetSize( inImg ), IPL_DEPTH_16S, 1 );	
	cvSobel( img_gray, drvX, 1, 0 );
	cvConvertScale( drvX, xsobel );
	cvSobel( img_gray, drvY, 0, 1 );
	cvConvertScale( drvY, ysobel );

	cvReleaseImage( &drvX );
	cvReleaseImage( &drvY );
	cvReleaseImage(&img_gray);

	/* Create an array of nbins images, one for each bin which will have zeroes for all pixels */
	IplImage** bins = (IplImage**) malloc(nbins * sizeof(IplImage*));
	for (int i = 0; i < nbins ; i++) {
		bins[i] = cvCreateImage(cvGetSize(inImg), IPL_DEPTH_32F,1);
		cvSetZero(bins[i]);
	}

	/* Create an array of nbins images (the cvIntegral() function requires image dimensions to be 64F),
	to store the integral images calculated from the above bin images */
	IplImage** integrals = (IplImage**) malloc(nbins * sizeof(IplImage*)); 
	for (int i = 0; i < nbins ; i++) {
		integrals[i] = cvCreateImage(cvSize(inImg->width + 1, inImg->height + 1),
			IPL_DEPTH_64F,1);
	}

	/* Calculate the bin images. The magnitude and orientation of the gradient at each pixel is 
	calculated using the xsobel and ysobel images. According to the orientation of the gradient, 
	the value of the corresponding pixel in the corresponding image is set */
	int x, y;
	float temp_gradient, temp_magnitude;
	for (y = 0; y <inImg->height; y++) {

		/* ptr1 and ptr2 point to beginning of the current row in the xsobel and ysobel images 
		respectively. ptrs[i] point to the beginning of the current rows in the bin images */
		float* ptr1 = (float*) (xsobel->imageData + y * (xsobel->widthStep));
		float* ptr2 = (float*) (ysobel->imageData + y * (ysobel->widthStep));
		float** ptrs = (float**) malloc(nbins * sizeof(float*));
		for (int i = 0; i < nbins ;i++){
			ptrs[i] = (float*) (bins[i]->imageData + y * (bins[i]->widthStep));
		}

		/*For every pixel in a row gradient orientation and magnitude are calculated and 
		corresponding values set for the bin images. */
		for (x = 0; x <inImg->width; x++) {

			/* if the xsobel derivative is zero for a pixel, a small value is added to it, 
			to avoid division by zero. atan returns values in radians, which on being converted
			to degrees, correspond to values between -90 and 90 degrees. 90 is added to each orientation, 
			to shift the orientation values range from {-90-90} to {0-180}. This is just a matter of convention */
			if (ptr1[x] == 0){
				temp_gradient = ((atan(ptr2[x] / (ptr1[x] + 0.00001))) * (180/ CV_PI)) + 90;
			}
			else{
				temp_gradient = ((atan(ptr2[x] / ptr1[x])) * (180 / CV_PI)) + 90;
			}
			temp_magnitude = sqrt((ptr1[x] * ptr1[x]) + (ptr2[x] * ptr2[x]));

			/*The bin image is selected according to the gradient values. The corresponding pixel
			value is made equal to the gradient magnitude at that pixel in the corresponding bin image */
			for (int i = 0; i < nbins; i++)
			{
				if (temp_gradient <= minDegree*(i+1))
				{
					ptrs[i][x] = temp_magnitude;
					break;
				}
			}
		}
	}

	cvReleaseImage(&xsobel);
	cvReleaseImage(&ysobel);

	/*Integral images for each of the bin images are calculated*/
	for (int i = 0; i <nbins ; i++){
		cvIntegral(bins[i], integrals[i]);
	}

	for (int i = 0; i <nbins ; i++){
		cvReleaseImage(&bins[i]);
	}

	return (integrals);
}

/* The following function takes as input the rectangular cell for which the HOG has to be calculated,
a matrix hog_cell of dimensions nbins to store the bin values for the integral histogram, and 
the normalization scheme to be used. No normalization is done if normalization = -1 */
void HOG::calculateHOG_rect(CvRect cell, CvMat* hog_cell, IplImage** integrals, int nbins, int normalization)
{

	/* Calculate the bin values for each of the bin of the histogram one by one */
	for (int i = 0; i < nbins ; i++){

		float a =((double*)(integrals[i]->imageData + (cell.y)
			* (integrals[i]->widthStep)))[cell.x];
		float b = ((double*) (integrals[i]->imageData + (cell.y + cell.height)
			* (integrals[i]->widthStep)))[cell.x + cell.width];
		float c = ((double*) (integrals[i]->imageData + (cell.y)
			* (integrals[i]->widthStep)))[cell.x + cell.width];
		float d = ((double*) (integrals[i]->imageData + (cell.y + cell.height)
			* (integrals[i]->widthStep)))[cell.x];

		((float*) hog_cell->data.fl)[i] = (a + b) - (c + d);

	}

	/*Normalize the matrix*/
	if (normalization != -1){
		cvNormalize(hog_cell, hog_cell, 1, 0, normalization);
	}
}

float HOG::ComputeHOGDistanceImg(IplImage* img1, CvRect rect1, IplImage* img2, CvRect rect2, int nbins)
{
	IplImage** integralHOG;
	vector<float> hog_window1;
	vector<float> hog_window2;
	CvMat* hog_cell = cvCreateMat(1,nbins,CV_32FC1);
	Utils *util = new Utils();
	IplImage* tempImg;	

	/* divide the rect1 into 2x2 cells */
	//int width_cell = (rect1.width - rect1.x) / 2;
	//int height_cell = (rect1.height - rect1.y) / 2;
	int width_cell = rect1.width / 2;
	int height_cell = rect1.height / 2;
	// up-left cell
	tempImg = cvCreateImage(cvSize(width_cell,height_cell),img1->depth,img1->nChannels);
	cvSetImageROI(img1,cvRect(rect1.x,rect1.y,width_cell,height_cell));	
	cvCopy(img1,tempImg);
	cvResetImageROI(img1);
	integralHOG = calculateIntegralHOG(tempImg, nbins);
	calculateHOG_rect(cvRect(0,0,tempImg->width,tempImg->height),hog_cell,integralHOG,nbins,1);
	for (int i = 0; i < hog_cell->cols; i++)
	{
		hog_window1.push_back((hog_cell->data.fl)[i]);
	}
	for (int i = 0; i <nbins ; i++){
		cvReleaseImage(&integralHOG[i]);
	}
	cvReleaseImage(&tempImg);

	// up-right cell
	tempImg = cvCreateImage(cvSize(width_cell,height_cell),img1->depth,img1->nChannels);
	cvSetImageROI(img1,cvRect(rect1.x + width_cell,rect1.y,width_cell,height_cell));	
	cvCopy(img1,tempImg);
	cvResetImageROI(img1);
	integralHOG = calculateIntegralHOG(tempImg, nbins);
	calculateHOG_rect(cvRect(0,0,tempImg->width,tempImg->height),hog_cell,integralHOG,nbins,1);
	for (int i = 0; i < hog_cell->cols; i++)
	{
		hog_window1.push_back((hog_cell->data.fl)[i]);
	}
	for (int i = 0; i <nbins ; i++){
		cvReleaseImage(&integralHOG[i]);
	}
	cvReleaseImage(&tempImg);

	// down-left cell
	tempImg = cvCreateImage(cvSize(width_cell,height_cell),img1->depth,img1->nChannels);
	cvSetImageROI(img1,cvRect(rect1.x,rect1.y+height_cell,width_cell,height_cell));	
	cvCopy(img1,tempImg);
	cvResetImageROI(img1);
	integralHOG = calculateIntegralHOG(tempImg, nbins);
	calculateHOG_rect(cvRect(0,0,tempImg->width,tempImg->height),hog_cell,integralHOG,nbins,1);
	for (int i = 0; i < hog_cell->cols; i++)
	{
		hog_window1.push_back((hog_cell->data.fl)[i]);
	}
	for (int i = 0; i <nbins ; i++){
		cvReleaseImage(&integralHOG[i]);
	}
	cvReleaseImage(&tempImg);

	// down-right cell
	tempImg = cvCreateImage(cvSize(width_cell,height_cell),img1->depth,img1->nChannels);
	cvSetImageROI(img1,cvRect(rect1.x+width_cell,rect1.y+height_cell,width_cell,height_cell));	
	cvCopy(img1,tempImg);
	cvResetImageROI(img1);
	integralHOG = calculateIntegralHOG(tempImg, nbins);
	calculateHOG_rect(cvRect(0,0,tempImg->width,tempImg->height),hog_cell,integralHOG,nbins,1);
	for (int i = 0; i < hog_cell->cols; i++)
	{
		hog_window1.push_back((hog_cell->data.fl)[i]);
	}
	for (int i = 0; i <nbins ; i++){
		cvReleaseImage(&integralHOG[i]);
	}
	cvReleaseImage(&tempImg);

	/* divide the rect 2 into 2x2 cells */
	//width_cell = (rect2.width - rect2.x) / 2;
	//height_cell = (rect2.height - rect2.y) / 2;
	width_cell = rect2.width / 2;
	height_cell = rect2.height / 2;
	// up-left cell
	tempImg = cvCreateImage(cvSize(width_cell,height_cell),img2->depth,img2->nChannels);
	cvSetImageROI(img2,cvRect(rect2.x,rect2.y,width_cell,height_cell));	
	cvCopy(img2,tempImg);
	cvResetImageROI(img2);
	integralHOG = calculateIntegralHOG(tempImg, nbins);
	calculateHOG_rect(cvRect(0,0,tempImg->width,tempImg->height),hog_cell,integralHOG,nbins,1);
	for (int i = 0; i < hog_cell->cols; i++)
	{
		hog_window2.push_back((hog_cell->data.fl)[i]);
	}
	for (int i = 0; i <nbins ; i++){
		cvReleaseImage(&integralHOG[i]);
	}
	cvReleaseImage(&tempImg);

	// up-right cell
	tempImg = cvCreateImage(cvSize(width_cell,height_cell),img2->depth,img2->nChannels);
	cvSetImageROI(img2,cvRect(rect2.x + width_cell,rect2.y,width_cell,height_cell));	
	cvCopy(img2,tempImg);
	cvResetImageROI(img2);
	integralHOG = calculateIntegralHOG(tempImg, nbins);
	calculateHOG_rect(cvRect(0,0,tempImg->width,tempImg->height),hog_cell,integralHOG,nbins,1);
	for (int i = 0; i < hog_cell->cols; i++)
	{
		hog_window2.push_back((hog_cell->data.fl)[i]);
	}
	for (int i = 0; i <nbins ; i++){
		cvReleaseImage(&integralHOG[i]);
	}
	cvReleaseImage(&tempImg);

	// down-left cell
	tempImg = cvCreateImage(cvSize(width_cell,height_cell),img2->depth,img2->nChannels);
	cvSetImageROI(img2,cvRect(rect2.x,rect2.y+height_cell,width_cell,height_cell));	
	cvCopy(img2,tempImg);
	cvResetImageROI(img2);
	integralHOG = calculateIntegralHOG(tempImg, nbins);
	calculateHOG_rect(cvRect(0,0,tempImg->width,tempImg->height),hog_cell,integralHOG,nbins,1);
	for (int i = 0; i < hog_cell->cols; i++)
	{
		hog_window2.push_back((hog_cell->data.fl)[i]);
	}
	for (int i = 0; i <nbins ; i++){
		cvReleaseImage(&integralHOG[i]);
	}
	cvReleaseImage(&tempImg);

	// down-right cell
	tempImg = cvCreateImage(cvSize(width_cell,height_cell),img2->depth,img2->nChannels);
	cvSetImageROI(img2,cvRect(rect2.x+width_cell,rect2.y+height_cell,width_cell,height_cell));	
	cvCopy(img2,tempImg);
	cvResetImageROI(img2);
	integralHOG = calculateIntegralHOG(tempImg, nbins);
	calculateHOG_rect(cvRect(0,0,tempImg->width,tempImg->height),hog_cell,integralHOG,nbins,1);
	for (int i = 0; i < hog_cell->cols; i++)
	{
		hog_window2.push_back((hog_cell->data.fl)[i]);
	}
	for (int i = 0; i <nbins ; i++){
		cvReleaseImage(&integralHOG[i]);
	}
	cvReleaseImage(&tempImg);

	// find distance
	float d = util->ComputeDistance(hog_window1,hog_window2);

	delete util;
	hog_window1.clear();
	hog_window2.clear();
	cvReleaseMat(&hog_cell);

	return d;
}